/**************************************************************************************************
  Filename:       heimdallr.c
  Revised:        $Date: 2013-08-15 15:28:40 -0700 (Thu, 15 Aug 2013) $
  Revision:       $Revision: 34986 $

  Description:    Watcher Heimdallr Application.
**************************************************************************************************/

#include <stdio.h>
#include <stdlib.h>

#include "bcomdef.h"
#include "OSAL.h"
#include "hal_drivers.h"
#include "OSAL_PwrMgr.h"

#include "OnBoard.h"

#include "gatt.h"

#include "hci.h"

#include "gapgattserver.h"
#include "gattservapp.h"
#include "gatt_profile_uuid.h"

#include "peripheral.h"
#include "gapbondmgr.h"

#include "devinfoservice.h"

#include "battprofile.h" 
#include "dataservice.h"

#include "osal_snv.h"
#include "OSAL_Clock.h"
#include "hal_adc.h"
#include "hal_flash.h"
#include "heimdallr.h"
#include "hal_uart.h"
#include "hal_sht.h"
#include "hal_mpu.h"
#include "hal_dbflash.h"
#include "bsp_storage.h"
#include "logic_handler.h"
#include "misc_func.h"

#include "hal_lis3dh.h"
#if defined FEATURE_OAD
  #include "oad.h"
  #include "oad_target.h"
#endif
/*********************************************************************
 * MACROS
 */
#define __debug__ 1
/*********************************************************************
 * CONSTANTS
 */


#define INITALIZED_TIME_VALUE                   0x1C1D89CF


/*
// keyfobProximityState values
#define HEIMDALLR_PROXSTATE_INITIALIZED          0   // Advertising after initialization or due to terminated link
#define HEIMDALLR_PROXSTATE_CONNECTED_IN_RANGE   1   // Connected and "within range" of the master, as defined by
                                                  // proximity profile
#define HEIMDALLR_PROXSTATE_PATH_LOSS            2   // Connected and "out of range" of the master, as defined by
                                                  // proximity profile
#define HEIMDALLR_PROXSTATE_LINK_LOSS            3   // Disconnected as a result of a supervision timeout
*/
// Company Identifier: Texas Instruments Inc. (13)
#define TI_COMPANY_ID                         0x000D

#define INVALID_CONNHANDLE                    0xFFFF

#if defined ( PLUS_BROADCASTER )
  #define ADV_IN_CONN_WAIT                    500 // delay 500 ms
#endif


//#define DEBUG 

//#define HAL_TEMP_TEST 
//#define HAL_ADC_TEST
//#define HAL_I2C_TEST
/*********************************************************************
 * TYPEDEFS
 */

/*********************************************************************
 * GLOBAL VARIABLES
 */
#ifdef __debug__
  // Sleep Interval
  uint32 FETUS_INTEVAL = 20800;
  // Humi/Temp Interval
  uint32 HUMI_INTEVAL = 15000;
  // Time update Inteval
  uint32 TIME_INTEVAL = 10000;
  // Update Sector 0A Data
  uint32 UPDATE_INTEVAL =35200;
#endif
#ifdef __production__
  // Sleep Interval
  uint32 FETUS_INTEVAL = 900000;
  // Humi/Temp Interval
  uint32 HUMI_INTEVAL = 300000;
  // Time update Inteval
  uint32 TIME_INTEVAL = 120000;
  // Update Sector 0A Data
  uint32 UPDATE_INTEVAL = 120000;
#endif
/*********************************************************************
 * EXTERNAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL FUNCTIONS
 */

/*********************************************************************
 * LOCAL VARIABLES
 */
static uint8 HeimDallr_TaskID;   // Task ID for internal task/event processing

static gaprole_States_t gapProfileState = GAPROLE_INIT;



/*********************************************************************
 * LOCAL FUNCTIONS
 */
static void HeimDallr_ProcessOSALMsg( osal_event_hdr_t *pMsg );
static void peripheralStateNotificationCB( gaprole_States_t newState );
//static void proximityAttrCB( uint8 attrParamID ); 
static void dataChangeCB(uint8 paramID); 

static void heimPairStateCB( uint16 connHandle, uint8 state, uint8 status );
static void WatcherHeimdallr_HandleSerial(mtOSALSerialData_t *cmdMsg);
//static void ConvertUint32ToUint8(uint32 value,uint8 *result);
//static void ConvertUint16ToUint8(uint32 value,uint8 *result);
//WatchDog
void Init_Watchdog(void);
void FeedDog(void);

//void hex2str( uint8 *pBuf,char *str,uint8 buflen);
//void displayTime(void);
/*********************************************************************
 * PROFILE CALLBACKS
 */

// GAP Role Callbacks
static gapRolesCBs_t heimDallr_PeripheralCBs =
{
  peripheralStateNotificationCB,  // Profile State Change Callbacks
  NULL                // When a valid RSSI is read from controller
};

// GAP Bond Manager Callbacks
static gapBondCBs_t heimDallr_BondMgrCBs =
{
  NULL,                     // Passcode callback (not used by application)
  heimPairStateCB                      // Pairing / Bonding state Callback (not used by application)
};

static dataCBs_t heimDallr_dataCBs =
{
  dataChangeCB,          // Called when Enabler attribute changes
};

typedef enum _fifo_ave{
  H_FIFO   =   0x01,
  T_FIFO   =   0x00  
} fifo_ave_t;

uint8 fifo_humi[20] = {0};
uint8 fifo_temp[20] = {0};

volatile DataPage _dataPg;
volatile UserPage _userPg;
volatile SecuPage _secuPg;
volatile TimePage _timePg;
volatile SettingsPage _settingsPg;
volatile HardWareInfo _hardPg;
volatile uint32 __currentTimeUnix;
volatile uint32 __currentAddr = DATA_START;
volatile uint32 __iterAddr = DATA_START;

volatile uint8 __realtimeFlag = DONT_REALTIME;
volatile uint8 mpuISR;
volatile uint8 __eraseFlag = 0;
volatile uint8 __save2MemFlag = 0;

uint8 global_stack[264];
uint8 global_string[13];
uint8 global_setting_argv[2];

volatile uint8 global_statck_busy = 0;

uint8 is_humi_led_on = FALSE;


extern const uint32 dataSectorAddrs[DATA_SECTOR_COUNT];
#ifdef __debug__
void peripheralTest(void);
#endif
// float Eulerangles[6];
/*********************************************************************
 * PUBLIC FUNCTIONS
 */

/*********************************************************************
 * @fn      HeimDallr_Init
 *
 * @brief   Initialization function for the Heimdallr App Task.
 *          This is called during initialization and should contain
 *          any application specific initialization (ie. hardware
 *          initialization/setup, table initialization, power up
 *          notificaiton ... ).
 *
 * @param   task_id - the ID assigned by OSAL.  This ID should be
 *                    used to send messages and set timers.
 *
 * @return  none
 */
void HeimDallr_Init( uint8 task_id )
{
  HeimDallr_TaskID = task_id;
  //Init Uart use UART1 on port1 
  UartInit();
  RegisterForSerial(HeimDallr_TaskID);
  // UART init finished
  BSP_LED_INIT();
  //Power on self test
  enMPUInterrupt();
  BattLED_ON();
  SignalLED_ON();
  dbg("Hello");
#if 1
  SignalLED_OFF();
  WarnLED_OFF();
  BattLED_OFF();
#endif
  //while(1);
  SPI_Init();
  //DBChipErase();
  bsp_flashInitFlag();
  if (_hardPg.flag != HARDWARE_INITIALIZED)
  {
     osal_setClock(INITALIZED_TIME_VALUE);
     dbg("\n \nfirst Init\n");
     bsp_dataPgInit();
     bsp_userPgInit();
     bsp_secuInit();
     bsp_timeSave();
     bsp_hardwareInit();
     bsp_updateTime();
  }
  else
  {
    dbg("\n \nnot first start");
    bsp_mem2dataPg();
    bsp_mem2userPg();
    bsp_mem2secuPg();
    bsp_mem2setPg();
    bsp_updateHardware();
    bsp_updateTime();
  }
  osal_memset(fifo_humi,0,20);
  osal_memset(fifo_temp,0,20);
  lis_startup();
  // peripheralTest();
//  angle_test();
  // uint32 diaper_interval;
  // uint32 diaper_number;
  // uint32 diaper_factor;
  // uint32 sleep_interval;
  // uint32 sleep_factor;
  // uint32 motion_interval;
  // uint32 motion_fator;
  // uint32 batt_interval;
  // uint32 batt_factor;
  dbg("update flag is %d\n",_settingsPg.update_flag);
  if (_settingsPg.update_flag == 0xB5)
  {
      FETUS_INTEVAL = _settingsPg.sleep_interval;  
      HUMI_INTEVAL = _settingsPg.diaper_interval;
      TIME_INTEVAL = _settingsPg.intervalTime;
      UPDATE_INTEVAL = _settingsPg.update_interval;
  }

 /*
  P0DIR |= (1<<5);
  P0SEL &= ~(1<<5);
  P0_5 = 1;  */
  /*  
  MPU_Init();
  //MPU_TEST();MPU_TEST();MPU_TEST();MPU_TEST();
  while(1)
  {
    //MPU_Select();
     getYawPitchRoll((float*)Eulerangles);          
     dbg("angle is-----%f-------%f--------%f-------\n\n",Eulerangles[0],Eulerangles[1],Eulerangles[2]); 
          
  
    uint8 acc[6];
    getACCEL8(acc);
    uint16 tmp = getTemp();
    dbg("-----%x,%x,%x, %x,%x,%x, %lx---\n",acc[0],acc[1],acc[2],acc[3],acc[4],acc[5],tmp);
  
    IsDeviceShaking();    
    WAIT_MS(2000);
  } 
  */
  /* */
  //MPU_TEST();
  //uint8 result[8];
  //SleepProcess(result);
  //motionProcess();

  //Init_Watchdog();  
 
  // Enable stack to toggle bypass control on TPS62730 (DC/DC converter)
/***********************************************
  * Before doing VDD measurement. You should:
  * 1)      disable PM IO port by: 
  * HCI_EXT_MapPmInOutPortCmd(HCI_EXT_PM_IO_PORT_NONE, HCI_EXT_PM_IO_PORT_PIN2)
  * 2)      Set P1.2 to low by P1DIR |= 0x04;P1 |= 0x04
  * 3)      Check battery with battMeasure()
  * 4)      Set P1.2 to high
  * 5)      Enable PM IO port by:
  * HCI_EXT_MapPmInOutPortCmd(HCI_EXT_PM_IO_PORT_P1, HCI_EXT_PM_IO_PORT_PIN2)
***********************************************/
  //HCI_EXT_MapPmIoPortCmd( HCI_EXT_PM_IO_PORT_P1, HCI_EXT_PM_IO_PORT_PIN2 );
  //DCDC_SBIT = 0;
  //Need to be concerned
  
  // Setup a delayed profile startup
#ifdef __debug__
  osal_start_timerEx( HeimDallr_TaskID, MWH_START_DEVICE_EVT, STARTDELAY );
#endif

  
}
void fifo_push(uint16 data,fifo_ave_t type)
{
  static uint8 idxh = 0;
  static uint8 idxt = 0;
  if (type == H_FIFO)
  {
    fifo_humi[idxh] = data;
    idxh = idxh < 19 ? idxh + 1 : 0;
  }
  if (type == T_FIFO)
  {
    fifo_temp[idxt] = data;
    idxt = idxt < 19 ? idxt + 1 : 0;
  }
}
uint16 fifo_move_average(fifo_ave_t type)
{
  uint8 count = 0;
  uint8 *fifo = type == H_FIFO ? fifo_humi : fifo_temp;
  uint32  sum = 0;
  for (uint8 i = 0; i < 20; ++i)
  {
    if (fifo[i] != 0)
    {
      sum += fifo[i];
      count ++ ;
    }
  }
  return (uint16)(sum / count);
} 
/*********************************************************************
 * @fn      HeimDallr_ProcessEvent
 *
 * @brief   Heimdallr Application Task event processor.  This function
 *          is called to process all events for the task.  Events
 *          include timers, messages and any other user defined events.
 *
 * @param   task_id  - The OSAL assigned task ID.
 * @param   events - events to process.  This is a bit map and can
 *                   contain more than one event.
 *
 * @return  none
 */
uint16 HeimDallr_ProcessEvent( uint8 task_id, uint16 events )
{
#ifdef __debug__
  // UTCTimeStruct tm;
  // osal_ConvertUTCTime(&tm,osal_getClock());
  // //dbg("\nt:%d/%d/%d %d:%d:%d\n",tm.year,tm.month+1,tm.day+1,tm.hour+1,tm.minutes+1,tm.seconds+1);
  // UnitData humiUnit;
  //   UnitData humiUnit;
  // uint8 humibuf[4] = {0x11,0xbd,0xcc,0x9e};
  // UDHumiInit(&humiUnit,humibuf);
  // dbg("\ndataHandler Test: %02x %08lx %02x %02x %02x %02x \n __currentAddr is: %08lx\n\n",humiUnit.meta,humiUnit.data[3],humiUnit.data[2],humiUnit.data[1],humiUnit.data[0],__currentAddr);
  // dbg("_cA:%08lx\n",__currentAddr);
  // dataHandler(humiUnit);
#endif  
  if ( events & SYS_EVENT_MSG )
  {
    uint8 *pMsg;
    if ( (pMsg = osal_msg_receive( HeimDallr_TaskID )) != NULL )
    {
      HeimDallr_ProcessOSALMsg( (osal_event_hdr_t *)pMsg );

      // Release the OSAL message
      VOID osal_msg_deallocate( pMsg );
    }

    // return unprocessed events
    return (events ^ SYS_EVENT_MSG);
  }

  if ( events & MWH_START_DEVICE_EVT )
  { 
    GAP_Setup();
#if defined FEATURE_OAD
  VOID OADTarget_AddService();                    // OAD Profile 
#endif
    // initialize the ADC for battery reads
    HalAdcInit();
     //LedBlink();
    
    // batteryMonitorInit();
    // Start the Device
    VOID GAPRole_StartDevice( &heimDallr_PeripheralCBs );

    // Start Bond Manager
    VOID GAPBondMgr_Register( &heimDallr_BondMgrCBs );
    
    // Set timer for first battery read event
    osal_start_timerEx( HeimDallr_TaskID, MWH_BATTERY_CHECK_EVT, BATTERY_CHECK_PERIOD );
    VOID Data_RegisterAppCBs( &heimDallr_dataCBs );
#if 1
    osal_start_timerEx( HeimDallr_TaskID, MWH_FETUS_CHECK_EVT, FETUS_INTEVAL );
    osal_start_timerEx( HeimDallr_TaskID, MWH_HUMI_READ_EVT, HUMI_INTEVAL );
    osal_start_timerEx( HeimDallr_TaskID, MWH_TIME_CHECK_EVT, TIME_INTEVAL );
    osal_start_timerEx( HeimDallr_TaskID, MWH_FLASH_UPDATE_EVT, UPDATE_INTEVAL );    
#endif
    // uint16 gapConnHandle;
    // uint8 gapRoleStat;
    // GAPRole_GetParameter( GAPROLE_CONNHANDLE, &gapConnHandle );
    // GAPRole_GetParameter( GAPROLE_STATE , &gapRoleStat);
    // if ( (gapRoleStat == GAPROLE_CONNECTED || gapRoleStat == GAPROLE_CONNECTED_ADV) && (gapConnHandle != INVALID_CONNHANDLE) )
    // {
    osal_start_timerEx( HeimDallr_TaskID, MWH_RESTART_SCAN_DEVICE, RESTART_SCAN_PERIOD );
    // }
    BattLED_ON();
    SignalLED_ON();
    // Set LED1 on to give feedback that the power is on, and a timer to turn off
    //HalLedSet( HAL_LED_1, HAL_LED_MODE_ON );
    //osal_pwrmgr_device( PWRMGR_ALWAYS_ON ); // To keep the LED on continuously.
    //osal_start_timerEx( HeimDallr_TaskID, MWH_POWERON_LED_TIMEOUT_EVT, 1000 );

    return ( events ^ MWH_START_DEVICE_EVT );
  }
  if ( events & MWH_CONNECTED_EVT)
  {
    dbg("\n\nconnected to do something\n");
    osal_start_timerEx( HeimDallr_TaskID, MWH_FETUS_CHECK_EVT, FETUS_INTEVAL );
    osal_start_timerEx( HeimDallr_TaskID, MWH_HUMI_READ_EVT, HUMI_INTEVAL );
    osal_start_timerEx( HeimDallr_TaskID, MWH_TIME_CHECK_EVT, TIME_INTEVAL );
    osal_start_timerEx( HeimDallr_TaskID, MWH_FLASH_UPDATE_EVT, UPDATE_INTEVAL );    
    return ( events ^ MWH_CONNECTED_EVT );
  }
  //Check update enable scan
  if ( events & MWH_RESTART_SCAN_DEVICE)
  {
    dbg("\n\n{gapProfileState:%x\n",gapProfileState);

    if (gapProfileState != GAPROLE_CONNECTED )
    {
      dbg("restarting123....\n");
      uint8 initial_advertising_enable = TRUE;
      // GAPRole_GetParameter( GAPROLE_ADVERT_ENABLED, &initial_advertising_enable );
      // // initial_advertising_enable = !initial_advertising_enable;
      // initial_advertising_enable = FALSE;
      // GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );  
      // initial_advertising_enable = TRUE;
      GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );  

    }
    dbg("gapProfileState:%x}\n\n",gapProfileState);
    if(RESTART_SCAN_PERIOD)
    {
      osal_start_timerEx( HeimDallr_TaskID, MWH_RESTART_SCAN_DEVICE, RESTART_SCAN_PERIOD );
    }
    return ( events ^ MWH_RESTART_SCAN_DEVICE );
  }
  
  // Humi data
  if ( events & MWH_HUMI_READ_EVT )
  {
#ifdef __debug__    
    dbg("humi");
#endif
    static uint8 _xCount = 1;
    static uint8 need_confirm = 0;
    uint8 every_minute_do = _xCount % 2 == 0 && (_xCount % 10 != 0); 
    uint8 twohalf_minutes_do  = _xCount % 3 == 0 ; 
    uint8 five_minutes_do = _xCount % 10 == 0;
    uint16 tmp_three_array[3];
    uint8 buf[3];
    dbg("==>tick tick %d,%d,%d<==",every_minute_do,twohalf_minutes_do,five_minutes_do);
    // uint8 bufh[3];            //Bit 1 of the two LSBs indicates the measurement type (‘0’: temperature, ‘1’ humidity). Bit 0 is currently not assigned.
    for (uint8 mi = 0; mi < 3; ++mi)
    {
       SHT_ReadTemp(buf);       //buft[0] is MSB, buft[1] is LSB with two lease significant bits is stat ,buft[3] is checksum   
       tmp_three_array[mi] = BUILD_UINT16(buf[0],buf[1]);
       WAIT_MS(10);
    }
    uint16 temperature = get_middle_one(tmp_three_array);
    
    for (uint8 mi = 0; mi < 3; ++mi)
    {
      SHT_ReadHumi(buf);     //// 
      tmp_three_array[mi] = BUILD_UINT16(buf[0],buf[1]);
      WAIT_MS(10);
    }
    uint16 humidity = get_middle_one(tmp_three_array);
    fifo_push(humidity,H_FIFO);
    fifo_push(temperature,T_FIFO);

    if ( every_minute_do && (need_confirm == NEED_CONFIRM_TURN_ON) &&\
         !is_humi_led_on )      // do confirm // LED ON
    {
      humidity = fifo_move_average(H_FIFO);
      temperature = fifo_move_average(T_FIFO);
      uint8 confirmed = HumiProcess(humidity,temperature);
      if (confirmed == NEED_CONFIRM_TURN_ON)
      {
        SignalLedBlink();
        broadcastHanler(BC_HUMI_EVENT);    // Maybe don't need. if connected ,data has already sent.
        is_humi_led_on = TRUE;
      }

    }
    if (twohalf_minutes_do && is_humi_led_on)  // below // LED off
    {
      // here need to confirm changed ***
      need_confirm = HumiProcess(humidity,temperature); 
      if (need_confirm == NEED_CONFIRM_TURN_OFF)
      {
        SignalLedStop();

        is_humi_led_on = FALSE;
      }

    }
    if ( five_minutes_do )
    {
      humidity = fifo_move_average(H_FIFO);
      temperature = fifo_move_average(T_FIFO);
      UnitData humiUnit;
      uint8 humibuf[4] = { HI_UINT16(humidity),LO_UINT16(humidity),
                           HI_UINT16(temperature),LO_UINT16(temperature)};  // Todo on Android: clear status bits
      UDHumiInit(&humiUnit,humibuf);
      dataHandler(humiUnit);
      need_confirm = HumiProcess(humidity,temperature);   // Already clear status bits

      _xCount = 1;
      // handler
      // led estimate
      // set or reset need confirm
    }
    
    _xCount ++ ;

    // Read three time, get the middle size
    // get the one, if below the safe line,clean buffer,turn led off.

    // moving average 
    // calc average every ten minutes
    // get the level for five minutes,LED light
    // just record the average part

    //dbg(" \nTemp Data:%02x %02x %02x\n",bufh[0],bufh[1],bufh[2]);

    //Calculate data and light   
    // HumiProcess(humibuf);   // Already clear status bits


    // dbg("Humi Data:%02x %02x %02x\n",buf[0],buf[1],buf[2]);
    if(HUMI_INTEVAL)
    {
      osal_start_timerEx( HeimDallr_TaskID, MWH_HUMI_READ_EVT, HUMI_INTEVAL );
    } 

    return (events ^ MWH_HUMI_READ_EVT);
  }

  // Fetus Status 180s
  if ( events & MWH_FETUS_CHECK_EVT )
  {
    dbg("sleep posture\n");
    UnitData slepUnit;
    uint8 slepbuf[8];
    // SleepProcess(slepbuf);
    UDSlepInit(&slepUnit,slepbuf);
    dataHandler(slepUnit);

    // Restart timer
    if ( FETUS_INTEVAL )
    {
      osal_start_timerEx( HeimDallr_TaskID, MWH_FETUS_CHECK_EVT, FETUS_INTEVAL );
    }
    return (events ^ MWH_FETUS_CHECK_EVT);
  }
  if ( events & MWH_SYNC_DATA_EVT )
  {
    syncHandler(HeimDallr_TaskID,0); //重新连接
    return (events ^ MWH_SYNC_DATA_EVT);
  }
  // if ( events & MWH_SYNC_START_EVT)
  // {
  //   syncHandler(HeimDallr_TaskID);
  //   return ( events ^ MWH_SYNC_START_EVT );
  // }
  // Time Check 120s
  if ( events & MWH_TIME_CHECK_EVT )
  {
    // Restart timer
    if ( TIME_INTEVAL )
    {
      osal_start_timerEx( HeimDallr_TaskID, MWH_TIME_CHECK_EVT, TIME_INTEVAL );
    }
    bsp_timeSave();

    // Todo: Time Update 
    return (events ^ MWH_TIME_CHECK_EVT);
  }
  if (events & MWH_SETTING_NOTIFY_EVT)
  {
    settingNotify(HeimDallr_TaskID,global_stack,global_setting_argv[0],global_setting_argv[1]);
    return (events ^ MWH_SETTING_NOTIFY_EVT); 
  }
  // Seperate from FLASH_UPDATE EVT, because the frequece of this event 
  if ( events & MWH_INFO_SAVE_EVT)
  {
    if (__save2MemFlag & 0x01)
    {
      if (isFlashBusy())
      {
        WAIT_MS(20);
      }
      bsp_userPg2Mem();
      __save2MemFlag = __save2MemFlag & ~0x01;
    }
    if (__save2MemFlag & 0x02)
    {
      if (isFlashBusy())
      {
        WAIT_MS(20);
      }
      bsp_secuPg2Mem();
      __save2MemFlag = __save2MemFlag & ~0x02;
    }
    if (__save2MemFlag & 0x04)
    {
      if (isFlashBusy())
      {
        WAIT_MS(20);
      }
      bsp_setPg2Mem();
      __save2MemFlag = __save2MemFlag & ~0x04;
    }
    return events ^ MWH_INFO_SAVE_EVT;
  }
  // Flash Meta Data Update
  if ( events & MWH_FLASH_UPDATE_EVT )
  {
    // Todo: addr update
    if(isFlashBusy())
    {
       osal_start_timerEx( HeimDallr_TaskID, MWH_FLASH_UPDATE_EVT, 500 );
       return (events ^ MWH_FLASH_UPDATE_EVT);
    }

    _dataPg.iterAddr = __iterAddr;
    _dataPg.newAddr = __currentAddr;
    bsp_dataPg2Mem();
    dbg("\n\nstatus has saved!\n\n");
 
    if ( UPDATE_INTEVAL )
    {
      osal_start_timerEx( HeimDallr_TaskID, MWH_FLASH_UPDATE_EVT, UPDATE_INTEVAL );
    }
    return (events ^ MWH_FLASH_UPDATE_EVT);
  }
  // Battery Check  Every 10 mins
  if ( events & MWH_BATTERY_CHECK_EVT )
  {
    // Restart timer
    if ( BATTERY_CHECK_PERIOD )
    {
      osal_start_timerEx( HeimDallr_TaskID, MWH_BATTERY_CHECK_EVT, BATTERY_CHECK_PERIOD );
    }

    // perform battery level check
    // Batt_MeasLevel();
    BattHandler();
    
    return (events ^ MWH_BATTERY_CHECK_EVT);
  }
  if ( events & MWH_SECTOR_ERASE_EVT )
  {
    return (events ^ MWH_SECTOR_ERASE_EVT);
#ifdef __NEED_ERASE_ALL__
    if(isFlashBusy())
    {
       osal_start_timerEx( HeimDallr_TaskID, MWH_SECTOR_ERASE_EVT, ERASE_INTVAL );
       return (events ^ MWH_SECTOR_ERASE_EVT);
    }
    if(__eraseFlag < DATA_SECTOR_COUNT)
    {
        __eraseFlag ++ ;
        DBSectorErase(dataSectorAddrs[__eraseFlag],NONBLOCK);
        if(__eraseFlag == DATA_SECTOR_COUNT - 1)
        {
          __eraseFlag = 0;
          osal_stop_timerEx(HeimDallr_TaskID,MWH_SECTOR_ERASE_EVT);
          return (events ^ MWH_SECTOR_ERASE_EVT);
        }
        osal_start_timerEx( HeimDallr_TaskID, MWH_SECTOR_ERASE_EVT, ERASE_INTVAL );
    }
 
    return (events ^ MWH_SECTOR_ERASE_EVT);
#endif
  } 

  // Discard unknown events
  return 0;
}

/*********************************************************************
 * @fn      HeimDallr_ProcessOSALMsg
 *
 * @brief   Process an incoming task message.
 *
 * @param   pMsg - message to process
 *
 * @return  none
 */
static void HeimDallr_ProcessOSALMsg( osal_event_hdr_t *pMsg )
{ 
  switch ( pMsg->event )
  {
     case SERIAL_MSG:
      WatcherHeimdallr_HandleSerial( (mtOSALSerialData_t *)pMsg );
      break;
  }
}

static void WatcherHeimdallr_HandleSerial(mtOSALSerialData_t *cmdMsg)
{
  uint8 i,len,*str=NULL;   
  str=cmdMsg->msg;        
  len=*str;               
#if defined(DEBUG)
  performTest();
#endif
  for(i=1;i<=len;i++)
    HalUARTWrite(0,str+i,1 ); 
  HalUARTWrite(0,"\r\n",2); 
}

/*********************************************************************
 * @fn      peripheralStateNotificationCB
 *
 * @brief   Notification from the profile of a state change.
 *
 * @param   newState - new state
 *
 * @return  none
 */
static void peripheralStateNotificationCB( gaprole_States_t newState )
{
  uint16 connHandle = INVALID_CONNHANDLE;

  if ( gapProfileState != newState )
  {
    switch( newState )
    {
    case GAPROLE_STARTED:
      {
        dbg("a0.started\n");
        // Set the system ID from the bd addr
        uint8 ownAddress[B_ADDR_LEN];  
        uint8 systemId[DEVINFO_SYSTEM_ID_LEN];
        GAPRole_GetParameter(GAPROLE_BD_ADDR, ownAddress);
       
        // use 6 bytes of device address for 8 bytes of system ID value
        systemId[0] = ownAddress[0];  
        systemId[1] = ownAddress[1];  
        systemId[2] = ownAddress[2];  
  
        // set middle bytes to zero
        systemId[4] = 0x00;
        systemId[3] = 0x00;

        // shift three bytes up
        systemId[7] = ownAddress[5];
        systemId[6] = ownAddress[4];
        systemId[5] = ownAddress[3];
        DevInfo_SetParameter(DEVINFO_SYSTEM_ID, DEVINFO_SYSTEM_ID_LEN, systemId);
      }
      break;

    //if the state changed to connected, initially assume that keyfob is in range
    case GAPROLE_ADVERTISING:
      {
        dbg("a5.adv\n");
        // Visual feedback that we are advertising.
        //HalLedSet( HAL_LED_2, HAL_LED_MODE_ON );
        // dbg("1.adv\n");
      }
      break;
      
    //if the state changed to connected, initially assume that keyfob is in range      
    case GAPROLE_CONNECTED:
      {
        dbg("a4.connected\n");
        // set the proximity state to either path loss alert or in range depending
        // on the value of keyfobProxIMAlertLevel (which was set by proximity monitor)
        GAPRole_GetParameter( GAPROLE_CONNHANDLE, &connHandle );
        // Turn off LED that shows we're advertising
        //HalLedSet( HAL_LED_2, HAL_LED_MODE_OFF );
        // osal_start_timerEx( HeimDallr_TaskID, MWH_SYNC_DATA_EVT, 1000 );   //50 is temperary
      }
      break;

    case GAPROLE_WAITING:
      {
        // then the link was terminated intentionally by the slave or master,
        // or advertising timed out
        /*************
        * todo  Save data
        ************/
        dbg("a2.waiting\n");
        __realtimeFlag = DONT_REALTIME;
        
        // Change attribute value of Accelerometer Enable to FALSE
        //       Accel_SetParameter(ACCEL_ENABLER, sizeof(valFalse), &valFalse);
        // Stop the acceleromter
        //      accelEnablerChangeCB(); // SetParameter does not trigger the callback

      }
      break;
 
    case GAPROLE_WAITING_AFTER_TIMEOUT:
      {
        dbg("a1.timeout\n");
        // the link was dropped due to supervision timeout  
        // Change attribute value of Accelerometer Enable to FALSE
        //       Accel_SetParameter(ACCEL_ENABLER, sizeof(valFalse), &valFalse);
        // Stop the acceleromter
        //     accelEnablerChangeCB(); // SetParameter does not trigger the callback
      }
      break;

    default:
      dbg("a3\n");
      // do nothing
      break;
    }
  } 
  dbg("=ax\n");
  gapProfileState = newState;
}

static void dataChangeCB(uint8 paramID)
{
  // dbg("Data Changed:%02x\n",paramID);
  switch(paramID)
  {
    case DATA_PACK_CONFIG_NO_OP:
      dbg("::::06\n");
      break;
    case DATA_PACK_CONFIG_SET_NOTIFY:
      dbg("::::07\n");
      break;
    case DATA_PACK_RECEIVED:
      { 
        uint8 string[13];
        Data_GetParameter(DATA_PACK,string);
        if (string[0] == 0xB5 || string[0] == 0x5B)
        {
          infoHandler(HeimDallr_TaskID,string);
        }
        else  // use ud to process string
        {
          udStrHandler(string);
        }
      }
      break;
    case DATA_ACK_RECEIVED:
      {
        dbg("rev ack\n");
      }
      break;
    case DATA_CTRL_RECEIVED:
      {
        // osal_start_timerEx( HeimDallr_TaskID, MWH_BATTERY_CHECK_EVT, 6000 );
        uint8 datactrl;
        Data_GetParameter(DATA_CTRL,&datactrl);
        dbg("rcv:0x%02x\n\n",datactrl);
        if (datactrl == DATACTRL_SYNCREADY)
        {
          dbg("rev 0x22\n");
          osal_start_timerEx( HeimDallr_TaskID, MWH_SYNC_DATA_EVT, 100 );   //50 is temperary
        }
        // if (datactrl == DATACTRL_OPENREALTIME)
        {
          __realtimeFlag = OPEN_REALTIME;
        }

        if (datactrl == DATACTRL_GET_USERPAGE || datactrl == DATACTRL_GET_SECUPAGE || datactrl == DATACTRL_GET_SETTINGPAGE)
        {
          if (global_statck_busy)
          {
            uint8 ack = 0x97; // SETTING_GLOBAL_IS_BUSY
            Data_SetParameter(DATA_ACK,1,&ack);
            return;
          }
          startSendSetting(HeimDallr_TaskID,datactrl);
        }

        if (datactrl == DATACTRL_GOTO_OAD)
        {
#if defined HAL_IMAGE_B
      uint16 crc[2] = { 0x0000, 0xFFFF };
      uint16 addr = OAD_IMG_R_PAGE * OAD_FLASH_PAGE_MULT + OAD_IMG_CRC_OSET / HAL_FLASH_WORD_SIZE;
      HalFlashWrite(addr, (uint8 *)crc, 1);
      HAL_SYSTEM_RESET();
#endif
      
        }
      }
      break;
    default: 
      break;
  }
}


void Init_Watchdog(void) 
{ 
  WDCTL  = 0x00;         
  WDCTL |= 0x08;         
}

void FeedDog(void) 
{ 
  WDCTL = 0xa0;
  WDCTL = 0x50; 
}

HAL_ISR_FUNCTION(MPU_PORT0_ISR,P0INT_VECTOR)
{
  HAL_ENTER_ISR();
  P0IF = 0x00;        //Clear Interrupt pending
  if (P0IFG & 0x20) //P0.5 interrupt
  {
    mpuISR = 0x01;
    dbg("receive LIS3DH INT1 interrupt\n\n");
    // TODO : receive  interrupt
    
    P0IFG &= ~0x20;     // Clear Interrupt Flag
  }
  if (P0IFG & 0x40)
  {
    mpuISR = 0x02;
    dbg("receive LIS3DH INT2 interrupt\n\n");

    P0IFG &= ~0x40;     // Clear Interrupt Flag
  }
  HAL_EXIT_ISR();
  return;
}


void peripheralTest(void)
{ 
    uint8 buf[3];
    SHT_ReadTemp(buf);
    dbg(" \nTemp Data:%02x %02x %02x\n",buf[0],buf[1],buf[2]);
    SHT_ReadHumi(buf);
    dbg("Humi Data:%02x %02x %02x\n",buf[0],buf[1],buf[2]);
    WAIT_MS(100);
    uint8 ami;
    LIS3DH_GetWHO_AM_I(&ami);
    
    dbg("Accel data : %x\n",ami);
    DBTest();
}
// //LIS3DH INT1/INT2
// HAL_ISR_FUNCTION(LIS3DH_PORT1_ISR,P1INT_VECTOR)
// {
//   HAL_ENTER_ISR();
//   P1IF = 0x00;
//   if(P1IFG & 0x02)      //Port1.1       LIS3DH INT1
//   {
//     // we stop the Port1.1 INT 
//     //uint8 dir6d=0x00;
//     //LIS3DH_ReadReg(LIS3DH_INT1_SRC, &dir6d);
//       //LIS3DH_Get6DPosition(&dir6d);
//     //uprint("=");
//     //uprintEx(&dir6d,1);
//     //uprint("*");
//   }
  
//   if(P1IFG & 0x04)      //Port1.2       LIS3DH INT2
//   {
//     P1IEN &= ~0x04; 
//     P1IF = 0x00;
//     static uint8 click;
//   //  LIS3DH_GetClickResponse(&click);
// //    paramCallback cb = {Accel_SetParameter,Accel_AsmParameter,ACCEL_CLICK_ATTR, 1, &data_accel_6d_buf,&click};
// //    transferData(gapProfileState,&cb,flash_asm_flag,&ec);
//     uprint("=C=");
//     uprintEx(&click,1);
//     P1IEN |= 0x04; 
//   }
//   HAL_EXIT_ISR();
//   return;
// }

// //T3 Interrupt Done
// HAL_ISR_FUNCTION(TIMER3_OVF_ISR,T3_VECTOR)
// {
//   HAL_ENTER_ISR();
//   T3IF = 0;
//   P1DIR |= 0x80;
//   P1SEL &= ~0x80;
//   P1_7 = 0;
//   if(0x01 & T3OVFIF)
//   {
//     HalUARTWrite(0,"T3_OVF_ISR\r\n",osal_strlen("T3_OVF_ISR\r\n"));
//   }
//   if(0x01 & T3CH0IF)
//   {
//     HalUARTWrite(0,"T3_ISR\r\n",osal_strlen("T3_ISR\r\n"));
//   }
  
//   HAL_EXIT_ISR();
//   return;
// }


/*********************************************************************
 * @fn      pairStateCB
 *
 * @brief   Pairing state callback.
 *
 * @return  none
 */
static void heimPairStateCB( uint16 connHandle, uint8 state, uint8 status )
{
  dbg(">>>> pair ing\n");
  if ( state == GAPBOND_PAIRING_STATE_STARTED )
  {
    dbg( "Pairing started");
  }
  else if ( state == GAPBOND_PAIRING_STATE_COMPLETE )
  {
    if ( status == SUCCESS )
    {
      dbg( "Pairing success" );
    }
    else
    {
      dbg( "Pairing fail:%d\n", status);
    }
  }
  else if ( state == GAPBOND_PAIRING_STATE_BONDED )
  {
    if ( status == SUCCESS )
    {
      dbg( "Bonding success");
    }
  }
}